#include <stdio.h>
#include <math.h>

const double PI = 3.1415926;
const double TIME = 0.1;  // 采样时间

double distance;  // 飞行器与导弹距离,飞行器感知距离20km，导弹感知距离40km
double gravity = 9.81;  // 重力加速度

double last_Aerocraft_x, last_Aerocraft_y, last_Aerocraft_z;  // 上一时刻飞行器坐标
double last_Missile_x, last_Missile_y, last_Missile_z;  // 上一时刻导弹坐标
double last_angle_xy, last_angle_z;  // 上一时刻导弹角度

long simutime = 0;  // 运行时间
int hit_flag = 0;  // 击中标志位

struct MyStruct0
{
	double height;  // 高度 5-20 km
	double pos_x;	  // x 坐标
	double pos_y;   // y 坐标
	double speed; // 速度 400-800 m/s
	//--------------------------------
	double angle; // 角度 单位rad
}Aerocraft = { 10000,0,2000,400,0};

struct MyStruct1
{
	double height;  // 高度
	double pos_x;	  // x 坐标
	double pos_y;	  // y 坐标
	//--------------------------------
	double speed; // 速度 初速800 m/s
	int fire; // 3次点火推进
	int if_speedup;  // 0-不加速 1-加速
	double angle_xy;  // 水平角度
	double angle_z;  // 竖直角度
}Missile = { 0,0,0,800,3,0,1,1 };

// 初始化飞行器参数
void AerocraftInit(double x, double y, double h, double s)
{
	Aerocraft.pos_x = x;
	Aerocraft.pos_y = y;
	Aerocraft.height = h;
	Aerocraft.speed = s;
}

double getAerocraftX()
{
	return Aerocraft.pos_x;
}

double getAerocraftY()
{
	return Aerocraft.pos_y;
}

double getAerocraftZ()
{
	return Aerocraft.height;
}

double getMissileX()
{
	return Missile.pos_x;
}

double getMissileY()
{
	return Missile.pos_y;
}

double getMissileZ()
{
	return Missile.height;
}

// 计算导弹与飞行器之间距离
double getDistance()
{
	double temp_x = Aerocraft.pos_x - Missile.pos_x;
	double temp_y = Aerocraft.pos_y - Missile.pos_y;
	double temp_z = Aerocraft.height - Missile.height;
	double temp = temp_x * temp_x + temp_y * temp_y + temp_z * temp_z;
	distance = sqrt(temp);
	return distance;
}

// 飞行器轨迹
void AerocraftPath()
{
	if (distance < 20000)  // 感知到导弹
	{
		if (Missile.height - last_Missile_z < 0)  // 导弹无动力下坠
		{
			Aerocraft.pos_x = Aerocraft.pos_x + cos(Aerocraft.angle) * Aerocraft.speed * TIME;
			Aerocraft.pos_y = Aerocraft.pos_y + sin(Aerocraft.angle) * Aerocraft.speed * TIME;
		}
		else  // 导弹仍有动力
		{
			double temp_angle;  // 方位角
			temp_angle = atan2(Missile.pos_y - Aerocraft.pos_y, Missile.pos_x - Aerocraft.pos_x);
			double three_clock = Aerocraft.angle;  // 3点钟方向
			double nine_clock = Aerocraft.angle + 1.57*2;  // 9点钟方向
			//double three_clock = Aerocraft.angle - 1.57;  // 3点钟方向
			//double nine_clock = Aerocraft.angle + 1.57;  // 9点钟方向
			double temp_x = cos(temp_angle);
			double temp_y = sin(temp_angle);
			double three_x = temp_x - cos(three_clock);
			double three_y = temp_y - sin(three_clock);
			double nine_x = temp_x - cos(nine_clock);
			double nine_y = temp_y - sin(nine_clock);
			double three_dis = three_x * three_x + three_y * three_y;
			double nine_dis = nine_x * nine_x + nine_y * nine_y;
			if (three_dis > nine_dis)
			{
				// 逆时针
				Aerocraft.pos_x = Aerocraft.pos_x + cos(Aerocraft.angle) * Aerocraft.speed * TIME;
				Aerocraft.pos_y = Aerocraft.pos_y + sin(Aerocraft.angle) * Aerocraft.speed * TIME;
				// 更新角度
				Aerocraft.angle = Aerocraft.angle + 0.1 * TIME;
			}
			else
			{
				// 顺时针
				Aerocraft.pos_x = Aerocraft.pos_x + cos(Aerocraft.angle) * Aerocraft.speed * TIME;
				Aerocraft.pos_y = Aerocraft.pos_y + sin(Aerocraft.angle) * Aerocraft.speed * TIME;
				// 更新角度
				Aerocraft.angle = Aerocraft.angle - 0.1 * TIME;
			}
		}
	}
	else    // 没有感知到导弹
	{
		Aerocraft.pos_x = Aerocraft.pos_x + cos(Aerocraft.angle) * Aerocraft.speed * TIME;
		Aerocraft.pos_y = Aerocraft.pos_y + sin(Aerocraft.angle) * Aerocraft.speed * TIME;
	}
	Aerocraft.angle = Aerocraft.angle > 6.2831852 ? Aerocraft.angle - 6.2831852 : Aerocraft.angle;
	Aerocraft.angle = Aerocraft.angle < 0 ? 6.2831852 + Aerocraft.angle : Aerocraft.angle;
}

// 导弹轨迹
void MissilePath()
{
	static int launch_flag = 0;  // 发射标志位
	double MissileVX = 0, MissileVY = 0, MissileVZ = 0;
	MissileVX = Missile.speed * cos(Missile.angle_z) * cos(Missile.angle_xy);
	MissileVY = Missile.speed * cos(Missile.angle_z) * sin(Missile.angle_xy);
	MissileVZ = Missile.speed * sin(Missile.angle_z);
	// 坐标更新
	Missile.pos_x = Missile.pos_x + MissileVX * TIME;
	Missile.pos_y = Missile.pos_y + MissileVY * TIME;
	Missile.height = Missile.height + MissileVZ * TIME;
	// 重力影响
	MissileVZ = MissileVZ - 9.81 * TIME;
	// 角度变化
	double MissileVXY = sqrt(MissileVX * MissileVX + MissileVY * MissileVY);
	Missile.angle_z = atan2(MissileVZ, MissileVXY);
	// 风阻影响
	Missile.speed = sqrt(MissileVXY * MissileVXY + MissileVZ * MissileVZ);
	Missile.speed = Missile.speed - 0.02 * Missile.speed * TIME;
	// 加速
	static int speedup = 0; // 2s加速时间
	if (Missile.if_speedup == 1 && Missile.fire!=0) // 加速
	{
		if (speedup < 20)
		{
			Missile.speed = Missile.speed + 100 * TIME;
			speedup++;
		}
		else
		{
			Missile.fire = Missile.fire - 1;
			speedup = 0;
			Missile.if_speedup = 0;
		}
	}
}

// 导弹导引律
void MissileControl() 
{
	//double K = 0.2;
	double angleXY, angleZ, last_angleXY, last_angleZ;
	double y0 = last_Aerocraft_y - last_Missile_y;
	double x0 = last_Aerocraft_x - last_Missile_x;
	last_angleXY = atan2(y0, x0);  // 上一时刻目标视线水平方位角
	last_angleZ = atan2(last_Aerocraft_z - last_Missile_z, sqrt(y0 * y0 + x0 * x0));  // 上一时刻目标视线竖直方位角

	double y1 = Aerocraft.pos_y - Missile.pos_y;
	double x1 = Aerocraft.pos_x - Missile.pos_x;
	angleXY = atan2(y1, x1);  // 当前时刻目标视线水平方位角
	angleZ = atan2(Aerocraft.height - Missile.height, sqrt(y1 * y1 + x1 * x1));
	Missile.angle_xy = (angleXY - last_angleXY) + Missile.angle_xy;
	Missile.angle_z = (angleZ - last_angleZ) + Missile.angle_z;
}

void control() // 总控制
{
	static double max_dis=0;
	distance = getDistance();
	max_dis = distance > max_dis ? distance : max_dis;
	if (distance < 50)  // 杀伤范围内
	{
		if (hit_flag == 0)
		{
			hit_flag = 1;
		}
		return;
	}
	last_Aerocraft_x = Aerocraft.pos_x;
	last_Aerocraft_y = Aerocraft.pos_y;
	last_Aerocraft_z = Aerocraft.height;
	last_Missile_x = Missile.pos_x;
	last_Missile_y = Missile.pos_y;
	last_Missile_z = Missile.height;
	last_angle_xy = Missile.angle_xy;
	last_angle_z = Missile.angle_z;
	if (simutime == 0) // 开始段
	{
		double y = Aerocraft.pos_y - Missile.pos_y;
		double x = Aerocraft.pos_x - Missile.pos_x;
		double xy = sqrt(y * y + x * x);
		Missile.if_speedup = 1;
		Missile.angle_xy = atan2(y, x);
		Missile.angle_z = atan2(Aerocraft.height - Missile.height, xy);
	}
	if (distance < 0.5 * max_dis && Missile.if_speedup == 0)  // 一半路程且未加速
	{
		Missile.if_speedup = 1;
		max_dis = distance;
	}
	if (Missile.height - last_Missile_z < 10 && Missile.fire != 0) // 竖直方向速度分量小于100
	{
		Missile.if_speedup = 1;
		max_dis = distance;
	}
	MissilePath();
	AerocraftPath();
	MissileControl();
	simutime++;
}

void showinfo()
{
	if (hit_flag == 0)
	{
		printf("未被击中\n");
	}
	else
	{
		printf("已被击中\n");
	}
	printf("当前导弹速度%f\n", Missile.speed);
	printf("当前飞行器速度%f\n", Aerocraft.speed);
	printf("当前飞行器高度%f\n", Aerocraft.height);
	printf("导弹加速次数%d\n", 3 - Missile.fire);
	printf("模拟时间长度%d秒\n", simutime/10);
}